/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <assert.h>
#include "CGLHeightMap.h"


CGLHeightMap::CGLHeightMap(int _Nx, int _Ny, float reso,
													 bool doTexturing, const char *texturename){
		if (doTexturing)
      sprintf(texname,"%s",texturename);
		Nx=_Nx;
		Ny=_Ny;
		resolution = reso;
		doTexture = doTexturing;
		displayListID = -1;
		texLoaded = false;
		heightMap=NULL;
		normalMap=NULL;
		dataMutex = ownMutex_Init();
		newData = false;
}

CGLHeightMap::~CGLHeightMap(){
		freeAll();
		ownMutex_Destroy(dataMutex);
}
/**
* Call this when you set the data from receiver thread!
*/
void CGLHeightMap::setData(float *hm){
	ownMutex_Lock(dataMutex);
	if(heightMap == NULL) allocate();
	fprintf(stderr,"SET (%d,%d)",Nx,Ny);
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			heightMap[i][j] = hm[j*Nx+i];
		}
	}
	newData = true;
	ownMutex_Unlock(dataMutex);
}


bool CGLHeightMap::generateDisplayList(){
		int step=1;
		int dID;
		if(!newData) return true;
		newData = false;
		ownMutex_Lock(dataMutex);
		for(int i=0;i<Nx;i++){
				for(int j=0;j<Ny;j++){
						normalMap[i][j]=hm_constructNormalForHeightMapPoint(heightMap,i, j,
																																		resolution, 1.0f,resolution,Nx,Ny);
				}
		}


		if(displayListID != -1){
				glDeleteLists(displayListID,1);
				displayListID = -1;
		}
		dID = glGenLists(1);

		if (dID == 0){
				fprintf(stderr,"CGLHeightMap::generateDisplayList::List id is Zero Error='%s'\n",
																																						gluErrorString(glGetError()));
				ownMutex_Unlock(dataMutex);
				return false;
		}

		displayListID = dID;
		setMaterialParameters();
		normalVectorList.clear();

		///FIXME: Nx or Nx-1?
		int xb=0,zb=0,xe=Nx-1,ze=Ny-1;
		for(int ix=xb; ix <= xe-step; ix += step) {
				glBegin(GL_TRIANGLE_STRIP);
				for(int iz=zb; iz <= ze; iz += step) {
						if (doTexture) {
								glTexCoord2f(1.0, (iz%2) ? 1.0 : 0.0);
						}

						if (normalMap != NULL) {
								glNormal3f( normalMap[ix][iz].x,
														normalMap[ix][iz].y,
														normalMap[ix][iz].z);
						}
						n.x =ix*resolution;
						n.y =heightMap[ix][iz];
						n.z =iz*resolution;

						glVertex3f(n.x , n.y, n.z);

						int ixt = ix+step;

						if (doTexture) {
								glTexCoord2f(0.0, (iz%2) ? 1.0 : 0.0);
						}

						if (normalMap != NULL) {
								glNormal3f( normalMap[ixt][iz].x,
														normalMap[ixt][iz].y,
														normalMap[ixt][iz].z );
						}

						//// FIXMEE:: NO SCALING IF FLOATS
						n.x =ixt*resolution;
						n.y =heightMap[ixt][iz];
						n.z =iz*resolution;

						glVertex3f(n.x , n.y, n.z);
				}
				glEnd();
		}

		if (doTexture) {
				texUnbind();
		}
		glPopAttrib();
		glEndList();

		ownMutex_Unlock(dataMutex);
		return true;
}
void CGLHeightMap::draw(){
	if(newData) generateDisplayList();
		ownMutex_Lock(dataMutex);
		if(displayListID > 0){
				glPushMatrix();
				//glTranslatef(x, y, z);
				//glTranslatef(-10, 0, -10);
				glCallList(displayListID);
				glPopMatrix();
		}
		ownMutex_Unlock(dataMutex);

}

/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
/// PRIVATE
/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
void CGLHeightMap::setMaterialParameters(){
		GLfloat amb[4] = {0.5, 0.5, 0.5, 0.5};
		GLfloat diff[4] = {0.5, 0.5, 0.5, 0.5};
		GLfloat glfModelSpecular[4];
		if (doTexture) {
				texLoad();
		}
		glNewList(displayListID, GL_COMPILE);
		glPushAttrib(GL_COLOR_BUFFER_BIT | GL_ENABLE_BIT);

		glEnable(GL_LIGHTING);
		glEnable(GL_CULL_FACE); // Cull the "frontside" of this object
		glCullFace(GL_FRONT);

		glEnable(GL_LIGHT1); // FIXME?

/*
		glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, amb);
		glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, diff);
	*/
		glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 30.0);

	/*
		glfModelSpecular[0] = 1;
		glfModelSpecular[1] = 1;
		glfModelSpecular[2] = 1;
		glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, glfModelSpecular);
	*/
		if (doTexture) {
				texBind();
		}
}



void CGLHeightMap::texUnbind()
{
		glDisable(GL_TEXTURE_2D);
}

void CGLHeightMap::texBind(){
		glEnable(GL_TEXTURE_2D);						// Enable Texture Mapping
		glBindTexture(GL_TEXTURE_2D, texture[0]);
		glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE );
}

void CGLHeightMap::texLoad(){
  // Set as UNPACK_ALIGNMENT
		if(!texLoaded){
				glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
				assert(glGetError() == GL_NO_ERROR);

				glGenTextures(1, &texture[0]);			// Create One Textures
				assert(glGetError() == GL_NO_ERROR);

				glBindTexture(GL_TEXTURE_2D, texture[0]);
				glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
				glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
				glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
				glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
				assert(glGetError() == GL_NO_ERROR);
				int r = LoadRGBMipmaps(texname, GL_RGB);
				assert (r == GL_TRUE);
				texLoaded = true;
		}
}


void CGLHeightMap::allocate(){
		freeAll();
		heightMap = new float * [Nx];
		for(int i=0; i < Nx; i++) {
				heightMap[i] = new float [Ny];
		}
		 /// Reserve memory for the Normal map
		normalMap = new TGraphVector* [Nx];
		for(int i=0; i < Nx; i++) {
				normalMap[i] = new TGraphVector [Ny];
		}

}
void CGLHeightMap::freeAll(){
		//FREE MEMORY
		if (normalMap != NULL) {
				for(int i=0; i < Nx; i++) {
						delete[] normalMap[i];
				}
				delete[] normalMap;
				normalMap = NULL;
		}

		if (heightMap != NULL) {
				for(int i=0; i < Nx; i++) {
						delete[] heightMap[i];
				}
				delete[] heightMap;
				heightMap = NULL;
		}
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
/// EXTRA HM FUNCTIONS
/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////

#define LIMITX(vv) (MIN(MAX(vv, 0), hm_dimX-1))
#define LIMITZ(vv) (MIN(MAX(vv, 0), hm_dimZ-1))
#define PT_X(px,pz) ((LIMITX(px))*xR)
#define PT_Y(px,pz) (heightMap[(LIMITX(px))][(LIMITZ(pz))]*yR)
#define PT_Z(px,pz) ((LIMITZ(pz))*zR)
#define SET_PT(pnp, qx, qz) pnp.x = PT_X(qx,qz); pnp.y = PT_Y(qx,qz); pnp.z = PT_Z(qx,qz);

TGraphVector hm_constructNormalForHeightMapPoint(float **heightMap,int x, int z,
				float xR, float yR,
				float zR,int xdim, int zdim)
{
		TGraphPoint p = { x*xR , heightMap[x][z] * yR, z*zR }; // 'Le point' ;)
		TGraphPoint np; // New point
		TGraphPointVector pv; // The neighbours point array
		int hm_dimX=xdim,hm_dimZ = zdim;

 // Add neighbour point coords to array
		SET_PT(np, x-1, z);  pv.push_back(np);
		SET_PT(np, x-1, z+1);  pv.push_back(np);
		SET_PT(np, x, z+1);  pv.push_back(np);
		SET_PT(np, x+1, z+1);  pv.push_back(np);
		SET_PT(np, x+1, z);  pv.push_back(np);
		SET_PT(np, x+1, z-1);  pv.push_back(np);
		SET_PT(np, x, z-1);  pv.push_back(np);
		SET_PT(np, x-1, z-1);  pv.push_back(np);

		return hm_calculateVertexNormal(p, pv); // Return the calculated normal
}

float hm_calculatePointDistance( float *p1, float *p2 )
{
		float
						a = (p2[X_AX] - p1[X_AX]),
		b = (p2[Y_AX] - p1[Y_AX]),
		c = (p2[Z_AX] - p2[Z_AX]);
#ifdef OS_WINDOWS
		return (float)sqrt((a*a)+(b*b)+(c*c));
#else
		return sqrtf((a*a)+(b*b)+(c*c)); // Faster
#endif //OS_WINDOWS
}
//******************************************************************************

TGraphVector hm_calculateVertexNormal(const TGraphPoint &vertex,
																			const TGraphPointVector &near_)
{
		TGraphVector n;
		TGraphTriangle tmpTriangle;

		n.x = 0.00;
		n.y = 0.00;
		n.z = 0.00;

 // Calculate sum of face normals
		tmpTriangle.a = vertex; // copy first triangle here
		for (vector<TGraphVector>::const_iterator i = near_.begin();
					 i != near_.end();
					 i++) {
							 tmpTriangle.b = *i;
							 i++;
							 tmpTriangle.c = *i;
							 n = n + hm_calculateFaceNormal(tmpTriangle);
					 }

 // Return normalized sum
					 return hm_normalizeVector(n);
}
//******************************************************************************

TGraphVector hm_calculateFaceNormal(const TGraphTriangle &t)
{
		TGraphVector v1 = t.b - t.a;
		TGraphVector v2 = t.c - t.a;
		TGraphVector ncp = hm_crossProductVector(v1, v2);
		TGraphVector vn = hm_normalizeVector(ncp);
		return vn;
}
//******************************************************************************

TGraphVector hm_normalizeVector(const TGraphVector &v)
{
		TGraphVector n;
		float l;

 // Calculate length, divide all components by length -> unit vector!
		l = sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
		n.x = v.x / l;
		n.y = v.y / l;
		n.z = v.z / l;

		return n;
}
//******************************************************************************

TGraphVector hm_crossProductVector(const TGraphVector &v1,
																	 const TGraphVector &v2)
{
 // Calculate cross product between the two 3 compo vectors
		TGraphVector n;
		n.x = v1.y * v2.z - v1.z * v2.y;
		n.y = v1.z * v2.x - v1.x * v2.z;
		n.z = v1.x * v2.y - v1.y * v2.x;
		return n;
}
//**********************************************************************//

//******************************************************************************/
bool operator == (const TGraphVector &a, const TGraphVector &b)
{
   //dPrint("%.5f %.5f %.5f == %.5f %.5f %.5f",a.x,a.y,a.z,b.x,b.y,b.z);
		if (fabs(a.x-b.x) > 0.0001) return false;
		if (fabs(a.y-b.y) > 0.0001) return false;
		if (fabs(a.z-b.z) > 0.0001) return false;
		return true;
}
//******************************************************************************

TGraphVector operator + (const TGraphVector &a, const TGraphVector &b)
{
		TGraphVector n;
		n.x = a.x + b.x;
		n.y = a.y + b.y;
		n.z = a.z + b.z;
		return n;
}
//******************************************************************************

TGraphVector operator - (const TGraphVector &a, const TGraphVector &b)
{
		TGraphVector n;
		n.x = a.x - b.x;
		n.y = a.y - b.y;
		n.z = a.z - b.z;
		return n;
}
//******************************************************************************
//******************************************************************************


/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
/// TEXTURE LOADING STUFF
/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef SEEK_SET
#  define SEEK_SET 0
#endif


/*
** RGB Image Structure
*/

typedef struct _TK_RGBImageRec {
   GLint sizeX, sizeY;
   GLint components;
   unsigned char *data;
} TK_RGBImageRec;



/******************************************************************************/

typedef struct _rawImageRec {
    unsigned short imagic;
    unsigned short type;
    unsigned short dim;
    unsigned short sizeX, sizeY, sizeZ;
    unsigned long min, max;
    unsigned long wasteBytes;
    char name[80];
    unsigned long colorMap;
    FILE *file;
    unsigned char *tmp, *tmpR, *tmpG, *tmpB, *tmpA;
    unsigned long rleEnd;
    GLuint *rowStart;
    GLint *rowSize;
} rawImageRec;

/******************************************************************************/

static void ConvertShort(unsigned short *array, long length)
{
   unsigned long b1, b2;
   unsigned char *ptr;

   ptr = (unsigned char *)array;
   while (length--) {
      b1 = *ptr++;
      b2 = *ptr++;
      *array++ = (b1 << 8) | (b2);
   }
}

static void ConvertLong(GLuint *array, long length)
{
   unsigned long b1, b2, b3, b4;
   unsigned char *ptr;

   ptr = (unsigned char *)array;
   while (length--) {
      b1 = *ptr++;
      b2 = *ptr++;
      b3 = *ptr++;
      b4 = *ptr++;
      *array++ = (b1 << 24) | (b2 << 16) | (b3 << 8) | (b4);
   }
}

static rawImageRec *RawImageOpen(const char *fileName)
{
   union {
      int testWord;
      char testByte[4];
   } endianTest;
   rawImageRec *raw;
   GLenum swapFlag;
   int x;

   endianTest.testWord = 1;
   if (endianTest.testByte[0] == 1) {
      swapFlag = GL_TRUE;
   } else {
      swapFlag = GL_FALSE;
   }

   raw = (rawImageRec *)malloc(sizeof(rawImageRec));
   if (raw == NULL) {
      fprintf(stderr, "Out of memory!\n");
      return NULL;
   }
   if ((raw->file = fopen(fileName, "rb")) == NULL) {
      perror(fileName);
      return NULL;
   }

   fread(raw, 1, 12, raw->file);

   if (swapFlag) {
      ConvertShort(&raw->imagic, 6);
   }

   raw->tmp = (unsigned char *)malloc(raw->sizeX*256);
   raw->tmpR = (unsigned char *)malloc(raw->sizeX*256);
   raw->tmpG = (unsigned char *)malloc(raw->sizeX*256);
   raw->tmpB = (unsigned char *)malloc(raw->sizeX*256);
   if (raw->sizeZ==4) {
      raw->tmpA = (unsigned char *)malloc(raw->sizeX*256);
   }
   if (raw->tmp == NULL || raw->tmpR == NULL || raw->tmpG == NULL ||
       raw->tmpB == NULL) {
      fprintf(stderr, "Out of memory!\n");
      return NULL;
   }

   if ((raw->type & 0xFF00) == 0x0100) {
      x = raw->sizeY * raw->sizeZ * sizeof(GLuint);
      raw->rowStart = (GLuint *)malloc(x);
      raw->rowSize = (GLint *)malloc(x);
      if (raw->rowStart == NULL || raw->rowSize == NULL) {
         fprintf(stderr, "Out of memory!\n");
         return NULL;
      }
      raw->rleEnd = 512 + (2 * x);
      fseek(raw->file, 512, SEEK_SET);
      fread(raw->rowStart, 1, x, raw->file);
      fread(raw->rowSize, 1, x, raw->file);
      if (swapFlag) {
         ConvertLong(raw->rowStart, x/sizeof(GLuint));
         ConvertLong((GLuint *)raw->rowSize, x/sizeof(GLint));
      }
   }
   return raw;
}

static void RawImageClose(rawImageRec *raw)
{

   fclose(raw->file);
   free(raw->tmp);
   free(raw->tmpR);
   free(raw->tmpG);
   free(raw->tmpB);
   if (raw->sizeZ>3) {
      free(raw->tmpA);
   }
   free(raw);
}

static void RawImageGetRow(rawImageRec *raw, unsigned char *buf, int y, int z)
{
   unsigned char *iPtr, *oPtr, pixel;
   int count;

   if ((raw->type & 0xFF00) == 0x0100) {
      fseek(raw->file, raw->rowStart[y+z*raw->sizeY], SEEK_SET);
      fread(raw->tmp, 1, (unsigned int)raw->rowSize[y+z*raw->sizeY],
            raw->file);

      iPtr = raw->tmp;
      oPtr = buf;
      while (1) {
         pixel = *iPtr++;
         count = (int)(pixel & 0x7F);
         if (!count) {
            return;
         }
         if (pixel & 0x80) {
            while (count--) {
               *oPtr++ = *iPtr++;
            }
         } else {
            pixel = *iPtr++;
            while (count--) {
               *oPtr++ = pixel;
            }
         }
      }
   } else {
      fseek(raw->file, 512+(y*raw->sizeX)+(z*raw->sizeX*raw->sizeY),
            SEEK_SET);
      fread(buf, 1, raw->sizeX, raw->file);
   }
}


static void RawImageGetData(rawImageRec *raw, TK_RGBImageRec *final)
{
   unsigned char *ptr;
   int i, j;

   final->data = (unsigned char *)malloc((raw->sizeX+1)*(raw->sizeY+1)*4);
   if (final->data == NULL) {
      fprintf(stderr, "Out of memory!\n");
   }

   ptr = final->data;
   for (i = 0; i < (int)(raw->sizeY); i++) {
      RawImageGetRow(raw, raw->tmpR, i, 0);
      RawImageGetRow(raw, raw->tmpG, i, 1);
      RawImageGetRow(raw, raw->tmpB, i, 2);
      if (raw->sizeZ>3) {
         RawImageGetRow(raw, raw->tmpA, i, 3);
      }
      for (j = 0; j < (int)(raw->sizeX); j++) {
         *ptr++ = *(raw->tmpR + j);
         *ptr++ = *(raw->tmpG + j);
         *ptr++ = *(raw->tmpB + j);
         if (raw->sizeZ>3) {
            *ptr++ = *(raw->tmpA + j);
         }
      }
   }
}


static TK_RGBImageRec *tkRGBImageLoad(const char *fileName)
{
   rawImageRec *raw;
   TK_RGBImageRec *final;

   raw = RawImageOpen(fileName);
   if (raw == NULL) return NULL;

   final = (TK_RGBImageRec *)malloc(sizeof(TK_RGBImageRec));
   if (final == NULL) {
      fprintf(stderr, "Out of memory!\n");
      return NULL;
   }
   final->sizeX = raw->sizeX;
   final->sizeY = raw->sizeY;
   final->components = raw->sizeZ;
   RawImageGetData(raw, final);
   RawImageClose(raw);
   return final;
}


static void FreeImage( TK_RGBImageRec *image )
{
   free(image->data);
   free(image);
}


/*
 * Load an SGI .rgb file and generate a set of 2-D mipmaps from it.
 * Input:  imageFile - name of .rgb to read
 *         intFormat - internal texture format to use, or number of components
 * Return:  GL_TRUE if success, GL_FALSE if error.
 */
GLboolean LoadRGBMipmaps( const char *imageFile, GLint intFormat )
{
   GLint error;
   GLenum format;
   TK_RGBImageRec *image;

   image = tkRGBImageLoad( imageFile );
   if (image == NULL) {
      return GL_FALSE;
   }

   if (image->components==3) {
      format = GL_RGB;
   }
   else if (image->components==4) {
      format = GL_RGBA;
   }
   else {
      /* not implemented */
      fprintf(stderr,
              "Error in LoadRGBMipmaps %d-component images not implemented\n",
              image->components );
      return GL_FALSE;
   }

   error = gluBuild2DMipmaps( GL_TEXTURE_2D,
                              format,
                              image->sizeX, image->sizeY,
                              intFormat,
                              GL_UNSIGNED_BYTE,
                              image->data );

   FreeImage(image);
   return error ? GL_FALSE : GL_TRUE;
}



/*
 * Load an SGI .rgb file and return a pointer to the image data.
 * Input:  imageFile - name of .rgb to read
 * Output:  width - width of image
 *          height - height of image
 *          format - format of image (GL_RGB or GL_RGBA)
 * Return:  pointer to image data or NULL if error
 */
GLubyte *LoadRGBImage( const char *imageFile, GLint *width, GLint *height,
                       GLenum *format )
{
   TK_RGBImageRec *image;
   GLint bytes;
   GLubyte *buffer;

   image = tkRGBImageLoad( imageFile );
   if (!image) {
      return NULL;
   }

   if (image->components==3) {
      *format = GL_RGB;
   }
   else if (image->components==4) {
      *format = GL_RGBA;
   }
   else {
      /* not implemented */
      fprintf(stderr,
              "Error in LoadRGBImage %d-component images not implemented\n",
              image->components );
      return NULL;
   }


   *width = image->sizeX;
   *height = image->sizeY;

   bytes = image->sizeX * image->sizeY * image->components;
   buffer = (GLubyte *) malloc(bytes);
   if (!buffer)
      return NULL;

   memcpy( (void *) buffer, (void *) image->data, bytes );

   FreeImage(image);

   return buffer;
}

